//
//  KITRobot.h
//  symbrion
//
//  Created by Berend Weel on 4/2/13.
//  Copyright (c) 2013 Berend Weel. All rights reserved.
//

#ifndef symbrion_KITRobot_h
#define symbrion_KITRobot_h

#include <boost/lexical_cast.hpp>

#include "KIT_noWheel.h"

class KIT : public KIT_noWheel {
public:
    KIT() :
        s1v(NULL),
        s2v(NULL),
        s3v(NULL),
        s4v(NULL),
        s1h(NULL),
        s2h(NULL),
        s3h(NULL),
        s4h(NULL) {
        
    }
    
    virtual void initialiseHardware() {
        KIT_noWheel::initialiseHardware();
        
        initialiseServos();
    }
    
private:
    void initialiseServos(){
        // Wheel Servo's
        s1v = getServo("servo1v");
		s1h = getServo("servo1h");
		s2v = getServo("servo2v");
		s2h = getServo("servo2h");
		s3v = getServo("servo3v");
		s3h = getServo("servo3h");
		s4v = getServo("servo4v");
		s4h = getServo("servo4h");
        
        s1h->setPosition(INFINITY);
        s2h->setPosition(INFINITY);
        s3h->setPosition(INFINITY);
        s4h->setPosition(INFINITY);
        
        s1h->setVelocity(0);
        s2h->setVelocity(0);
        s3h->setVelocity(0);
        s4h->setVelocity(0);
    }
    
    virtual void setLeftSpeed(double speed) {
        s1h->setVelocity(speed);
        s2h->setVelocity(speed);
    }
    
    virtual void setRightSpeed(double speed) {
        s3h->setVelocity(speed);
        s4h->setVelocity(speed);
    }
    
protected:
    // KIT Servo's
	Servo *s1v, *s1h; // Half 1
	Servo *s2v, *s2h; // Half 2
	Servo *s3v, *s3h; // Half 1
	Servo *s4v, *s4h; // Half 2
};

#endif
